import RPi.GPIO as GPIO
import time
import threading

# 导入电机控制模块
from motor_control import init_gpio as init_gpio_motor
from motor_control import PID, start_speed_monitor, \
    set_motor_speed, rotate_in_place_with_pid, drive_straight_with_pid, \
    drive_straight_with_color, cleanup as cleanup_motor

# 导入颜色检测模块
from detect_color import init_camera, start_color_detection, \
    get_latest_color_data, cleanup as cleanup_camera

# 导入超声波模块
from detect_distance import init_i2c, measure_distance, \
    start_distance_measurement, get_latest_distance, \
    cleanup as cleanup_distance


# 全局变量
global running, l_pid, r_pid, state, state_color, state_1_done, state_2_done
running = True
l_pid = None
r_pid = None
state = 1
state_color = {}  # 存储识别到的魔方颜色
state_1_done = False  # 状态1是否完成
state_2_done = False  # 状态2是否完成

# 颜色识别相关参数
COLOR_CONFIRM_COUNT = 3  # 需要连续识别相同颜色的次数
color_confirm_counter = {}  # 颜色确认计数器
confirmed_color = None  # 已确认的颜色

# 避障和导航参数
MIN_DISTANCE = 20  # 最小安全距离(cm)
BEGIN_DISTANCE = 30  # 开始避障距离(cm)
ON_TAGERT_TIME = 2  # 靠近目标后的绕目标旋转时间(秒)
SIDE_OFFSET = 50   # 侧向偏移像素值(用于绕过魔方)
STRAIGHT_TIME = 2  # 通过魔方后直行的时间(秒)

# 左转和右转的颜色分类
LEFT_COLORS = ["red", "yellow"]
RIGHT_COLORS = ["blue", "green"]

try:
    # 初始化电机
    pwma, pwmb = init_gpio_motor()
    print("电机已初始化")
    
    # 启动电机速度监测
    start_speed_monitor()
    print("电机速度监测已启动")

    start_distance_measurement()  # 启动超声波模块
    print("超声波模块已启动")
    
    # 初始化摄像头
    camera = init_camera() # 摄像头ID默认为0，可选输入摄像头ID
    if camera is None:
        print("摄像头初始化失败，程序退出")
        exit(1)
    
    # 启动颜色检测线程
    start_color_detection(interval=0.1)  # 每0.1秒检测一次
    print("颜色检测线程已启动")
    
    # 启动超声波模块
    # TODO: 超声波模块

    # 等待系统稳定
    print("系统初始化中，请稍候...")
    time.sleep(2)
    
    # 主控制循环
    while running:
        # 获取最新的颜色检测结果
        color_data = get_latest_color_data()

        # 获取超声波距离
        distance = get_latest_distance()
        #distance = 100  # 临时设置，实际应从超声波模块获取
        
        # 状态1：识别并通过第一个魔方
        if state == 1:
            print("当前状态: 1 - 识别并通过第一个魔方")
            
            # 如果没有检测到颜色，直行寻找魔方
            if not color_data:
                print("未检测到魔方，直行寻找...")
                l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed=1.5, l_pid=l_pid, r_pid=r_pid)
                time.sleep(0.1)
                continue
            
            # 如果已经确认了颜色，直接使用该颜色进行导航
            if confirmed_color:
                segments = color_data.get(confirmed_color, [])
                if segments:
                    # 先前漏识别了魔方，删除计时器
                    if hasattr(state, 'lost_target_time'):
                        delattr(state, 'lost_target_time')
                    segment = segments[0]  # 取第一个段
                    x_start, x_end, x_center = segment
                    
                    # TODO:根据距离判断行为
                    if distance < MIN_DISTANCE and abs(x_center) < 50:
                        # 距离太近，判断是否会碰撞
                        # 如果魔方在中间，可能会碰撞
                        print("魔方距离太近且在中间，倒车避让")
                        l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed=-1, l_pid=l_pid, r_pid=r_pid)
                        
                    else:
                        # 距离恰当，根据颜色选择绕行方向
                        distance < BEGIN_DISTANCE #and abs(x_center) < 70
                        target_offset = 0
                        
                        if confirmed_color in LEFT_COLORS:
                            # 左侧绕行，瞄准左边缘左侧
                            target_offset = x_start - SIDE_OFFSET
                            print(f"左侧绕行，目标偏移: {target_offset}")
                        elif confirmed_color in RIGHT_COLORS:
                            # 右侧绕行，瞄准右边缘右侧
                            target_offset = x_end + SIDE_OFFSET
                            print(f"右侧绕行，目标偏移: {target_offset}")
                        #采取静态拐弯，从探测到魔方到达一定距离后，再开始控制拐弯，向着着模仿中间加上偏离值行走n秒，然后直行
                        #效果以向右绕为例，如果小车当前面魔方中心，先右拐，再左拐环绕，这段时间内小车在围绕魔方圆心画圆
                        if not hasattr(state, 'on_target_1_time'):
                            state.on_target_1_time = time.time()
                        if time.time()-state.on_target_1_time < ON_TAGERT_TIME:
                            # 使用颜色偏移控制小车
                            l_pid, r_pid = drive_straight_with_color(pwma, pwmb, target_offset, target_speed=1.5, l_pid=l_pid, r_pid=r_pid)
                            time.sleep(0.1)
                        else:
                            # 到达预期绕行时间后，开始直行
                            delattr(state, 'on_target_1_time')
                            l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed=1.5, l_pid=l_pid, r_pid=r_pid)
                            time.sleep(0.2)
                        
                        # 检查是否已经通过魔方的逻辑不再使用位置判断
                else:
                    if distance < MIN_DISTANCE:
                        # 距离太近，判断是否会碰撞
                        # 如果魔方在中间，可能会碰撞
                        print("魔方距离太近且在中间，倒车避让")
                        l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed=-1, l_pid=l_pid, r_pid=r_pid)
                        time.sleep(0.1)
                        continue
                    # 已确认颜色但找不到该颜色的魔方
                    # 可能是已经绕过了魔方，开始计时
                    if not hasattr(state, 'lost_target_time'):
                        state.lost_target_time = time.time()
                        print(f"找不到已确认的{confirmed_color}魔方，开始计时...")
                        l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed=1, l_pid=l_pid, r_pid=r_pid)
                    else:
                        # 检查是否已经持续一段时间没有检测到目标魔方
                        if time.time() - state.lost_target_time > 1.5:  # 假设1.5秒没检测到就是通过了
                            print("持续一段时间未检测到魔方，认为已通过，继续直行一段时间")
                            # 重置计时器
                            delattr(state, 'lost_target_time')
                            
                            # 通过后直行一段时间
                            start_time = time.time()
                            while time.time() - start_time < STRAIGHT_TIME:
                                l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed=1.5, l_pid=l_pid, r_pid=r_pid)
                                time.sleep(0.1)
                            
                            # 完成状态1，进入状态2
                            state_1_done = True
                            state = 2
                            confirmed_color = None  # 重置确认的颜色
                            color_confirm_counter = {}  # 重置颜色计数器
                            print("进入状态2: 寻找两个同色魔方")
                        else:
                            # 还在计时中，继续直行
                            print(f"找不到已确认的{confirmed_color}魔方，继续寻找... ({time.time() - state.lost_target_time:.1f}秒)")
                            l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed=1, l_pid=l_pid, r_pid=r_pid)
            else:
                # 还没有确认颜色，需要进行颜色确认
                # 找到最宽的颜色块作为目标魔方
                widest_color = None
                max_width = 0
                
                for color, segments in color_data.items():
                    if segments:  # 确保有检测到该颜色
                        for segment in segments:  
                            width = abs(segment[1] - segment[0])  # 计算宽度
                            if width > max_width:
                                max_width = width
                                widest_color = color
                
                if widest_color:
                    # 颜色确认逻辑
                    if widest_color not in color_confirm_counter:
                        color_confirm_counter[widest_color] = 1
                    else:
                        color_confirm_counter[widest_color] += 1
                    
                    # 重置其他颜色的计数
                    for color in color_confirm_counter:
                        if color != widest_color:
                            color_confirm_counter[color] = 0
                    
                    # 检查是否已经连续确认足够次数
                    if color_confirm_counter.get(widest_color, 0) >= COLOR_CONFIRM_COUNT:
                        confirmed_color = widest_color
                        state_color[1] = confirmed_color  # 记录状态1的颜色
                        print(f"状态1: 确认魔方颜色为 {confirmed_color}")
                else:
                    # 没有找到足够宽的颜色块，继续直行
                    l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed=1, l_pid=l_pid, r_pid=r_pid)
        
        # TODO: 状态2和状态3的逻辑
        # 状态2：寻找并通过两个同色魔方
        elif state == 2:
            print("当前状态: 2 - 寻找并通过两个同色魔方")
            
            # 如果已经确认了颜色，直接使用该颜色进行导航
            if confirmed_color:
                segments = color_data.get(confirmed_color, [])
                if len(segments) == 2:
                    # 先前漏识别了魔方，删除计时器
                    if hasattr(state, 'lost_target_time'):
                        delattr(state, 'lost_target_time')
                    
                    # 计算两个魔方之间的中点
                    x1_start = segments[0][0]  # 第二个魔方的左边缘
                    x1_end = segments[0][1]    # 第一个魔方的右边缘
                    x2_start = segments[1][0]  # 第二个魔方的左边缘
                    x2_end = segments[1][1]  # 第二个魔方的右边缘
                    x1_width = x1_end - x1_start  # 第一个魔方的宽度
                    x2_width = x2_end - x2_start  # 第二个魔方的宽度
                    target_center = (x1_end + x2_start) // 2  # 两个魔方边缘之间的中点
                    ok_to_go_distance_bit = x2_start - x1_end  # 两个魔方边缘之间的像素距离
                    
                    # 根据距离判断行为
                    if distance < MIN_DISTANCE and abs(target_center) < 50:
                        # 距离太近，判断是否会碰撞
                        # 如果中点在中间，可能会碰撞
                        print("魔方距离太近且在中间，倒车避让")
                        l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed=-1, l_pid=l_pid, r_pid=r_pid)
                        # 除了退让，还要考虑是否要面向中心调整角度
                        time.sleep(2)
                        # 尝试调整角度
                        if x1_width > x2_width:
                            # 第一个魔方更宽，右拐
                            print("第一个魔方更宽，右拐")
                            rotate_in_place_with_pid(pwma, pwmb, 'clockwise', target_speed=1.5, l_pid=l_pid, r_pid=r_pid)
                            time.sleep(0.5)
                        else:
                            # 第二个魔方更宽，左拐
                            print("第二个魔方更宽，左拐")
                            rotate_in_place_with_pid(pwma, pwmb, 'counterclockwise', target_speed=1.5, l_pid=l_pid, r_pid=r_pid)
                            time.sleep(0.5)
                        
                    

                    else:
                        ok_to_go_distance_bit > 200
                        # 距离足够，且左右留出的中间路径足够宽，瞄准两个魔方之间的中点
                        print(f"瞄准两个魔方之间的中点: {target_center}")
                        l_pid, r_pid = drive_straight_with_color(pwma, pwmb, target_center, target_speed=1.5, l_pid=l_pid, r_pid=r_pid)
                        time.sleep(0.5)#直行盲走一段时间，穿过
                else:
                    # 已确认颜色但找不到两个该颜色的魔方
                    # 可能是已经绕过了魔方，开始计时
                    if not hasattr(state, 'lost_target_time'):
                        state.lost_target_time = time.time()
                        print(f"找不到两个{confirmed_color}魔方，开始计时...")
                        l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed=1, l_pid=l_pid, r_pid=r_pid)
                    else:
                        # 检查是否已经持续一段时间没有检测到目标魔方
                        if time.time() - state.lost_target_time > 1.5:  # 假设1.5秒没检测到就是通过了
                            print("持续一段时间未检测到两个同色魔方，认为已通过，继续直行一段时间")
                            # 重置计时器
                            delattr(state, 'lost_target_time')
                            
                            # 通过后直行一段时间
                            start_time = time.time()
                            while time.time() - start_time < STRAIGHT_TIME:
                                l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed=1.5, l_pid=l_pid, r_pid=r_pid)
                                time.sleep(0.1)
                            
                            # 完成状态2，进入状态3
                            state_2_done = True
                            state = 3
                            confirmed_color = None  # 重置确认的颜色
                            color_confirm_counter = {}  # 重置颜色计数器
                            print("进入状态3: 寻找最后一个魔方")
                        else:
                            # 还在计时中，继续直行
                            print(f"找不到两个{confirmed_color}魔方，继续寻找... ({time.time() - state.lost_target_time:.1f}秒)")
                            l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed=1, l_pid=l_pid, r_pid=r_pid)
            else:
                # 还没有确认颜色，需要进行颜色确认
                # 检查是否有两个相同颜色的魔方
                for color, segments in color_data.items():
                    if len(segments) == 2:  # 如果检测到两个相同颜色的段
                        # 颜色确认逻辑
                        if color not in color_confirm_counter:
                            color_confirm_counter[color] = 1
                        else:
                            color_confirm_counter[color] += 1
                        
                        # 重置其他颜色的计数
                        for c in color_confirm_counter:
                            if c != color:
                                color_confirm_counter[c] = 0
                        
                        # 检查是否已经连续确认足够次数
                        if color_confirm_counter.get(color, 0) >= COLOR_CONFIRM_COUNT:
                            confirmed_color = color
                            state_color[2] = confirmed_color  # 记录状态2的颜色
                            print(f"状态2: 确认两个同色魔方颜色为 {confirmed_color}")
                            break
                
                # 如果没有找到两个同色魔方，继续旋转
                if not confirmed_color:
                    print("未找到两个同色魔方，继续旋转寻找")
                    l_pid, r_pid = rotate_in_place_with_pid(pwma, pwmb, 'clockwise', target_speed=1, l_pid=l_pid, r_pid=r_pid)
        
        # 状态3：寻找并通过最后一个魔方
        elif state == 3:
            print("当前状态: 3 - 寻找并通过最后一个魔方")
            
            # 如果已经确认了颜色，直接使用该颜色进行导航
            if confirmed_color:
                segments = color_data.get(confirmed_color, [])
                if segments:
                    # 先前漏识别了魔方，删除计时器
                    if hasattr(state, 'lost_target_time'):
                        delattr(state, 'lost_target_time')
                    
                    segment = segments[0]  # 取第一个段
                    x_start, x_end, x_center = segment
                    
                    # 根据距离判断行为
                    if distance < MIN_DISTANCE and abs(x_center) < 50:
                        # 距离太近，判断是否会碰撞
                        # 如果魔方在中间，可能会碰撞
                        print("魔方距离太近且在中间，倒车避让")
                        l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed=-1, l_pid=l_pid, r_pid=r_pid)
                    else:
                        # 距离恰当，根据颜色选择绕行方向
                        distance < BEGIN_DISTANCE #and abs(x_center) < 70
                        # 距离足够，根据颜色选择绕行方向
                        target_offset = 0
                        
                        if confirmed_color in LEFT_COLORS:
                            # 左侧绕行，瞄准左边缘左侧
                            target_offset = x_start - SIDE_OFFSET
                            print(f"左侧绕行，目标偏移: {target_offset}")
                        elif confirmed_color in RIGHT_COLORS:
                            # 右侧绕行，瞄准右边缘右侧
                            target_offset = x_end + SIDE_OFFSET
                            print(f"右侧绕行，目标偏移: {target_offset}")
                        
                        # 使用颜色偏移控制小车
                        #l_pid, r_pid = drive_straight_with_color(pwma, pwmb, target_offset, target_speed=1.5, l_pid=l_pid, r_pid=r_pid)
                        
                        #采取静态拐弯，从探测到魔方到达一定距离后，再开始控制拐弯，向着着模仿中间加上偏离值行走n秒，然后直行
                        #效果以向右绕为例，如果小车当前面魔方中心，先右拐，再左拐环绕，这段时间内小车在围绕魔方圆心画圆
                        if not hasattr(state, 'on_target_1_time'):
                            state.on_target_1_time = time.time()
                        if time.time()-state.on_target_1_time < ON_TAGERT_TIME:
                            # 使用颜色偏移控制小车
                            l_pid, r_pid = drive_straight_with_color(pwma, pwmb, target_offset, target_speed=1.5, l_pid=l_pid, r_pid=r_pid)
                            time.sleep(0.1)
                        else:
                            # 到达预期绕行时间后，开始直行
                            delattr(state, 'on_target_1_time')
                            l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed=1.5, l_pid=l_pid, r_pid=r_pid)
                            time.sleep(0.2)
                else:
                    # 已确认颜色但找不到该颜色的魔方
                    # 可能是已经绕过了魔方，开始计时
                    if not hasattr(state, 'lost_target_time'):
                        state.lost_target_time = time.time()
                        print(f"找不到已确认的{confirmed_color}魔方，开始计时...")
                        l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed=1, l_pid=l_pid, r_pid=r_pid)
                    else:
                        # 检查是否已经持续一段时间没有检测到目标魔方
                        if time.time() - state.lost_target_time > 1.5:  # 假设1.5秒没检测到就是通过了
                            print("持续一段时间未检测到魔方，认为已通过，任务完成！")
                            # 重置计时器
                            delattr(state, 'lost_target_time')
                            
                            print(f"识别到的魔方颜色: 状态1={state_color.get(1)}, 状态2={state_color.get(2)}, 状态3={state_color.get(3)}")
                            
                            # 停止小车
                            set_motor_speed(pwma, pwmb, 0, 0)
                            running = False
                        else:
                            # 还在计时中，继续直行
                            print(f"找不到已确认的{confirmed_color}魔方，继续寻找... ({time.time() - state.lost_target_time:.1f}秒)")
                            l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed=1, l_pid=l_pid, r_pid=r_pid)
            else:
                # 修改状态3的转向逻辑
                # 如果还没有找到最后一个魔方，执行转向操作
                if not hasattr(state, 'rotation_started'):
                    # 初始化旋转状态
                    state.rotation_started = True
                    state.rotation_phase = 'left'  # 先左转
                    state.rotation_start_time = time.time()
                    print("开始执行转向操作寻找最后一个魔方")
                
                # 根据旋转阶段执行不同的旋转
                if state.rotation_phase == 'left':
                    # 左转90度
                    if time.time() - state.rotation_start_time < 1.5:  # 假设1.5秒可以转90度
                        print("左转寻找魔方...")
                        l_pid, r_pid = rotate_in_place_with_pid(pwma, pwmb, 'counterclockwise', target_speed=1.5, l_pid=l_pid, r_pid=r_pid)
                    else:
                        # 左转完成，开始右转
                        state.rotation_phase = 'right'
                        state.rotation_start_time = time.time()
                        print("左转完成，开始右转寻找魔方...")
                elif state.rotation_phase == 'right':
                    # 右转180度
                    if time.time() - state.rotation_start_time < 3:  # 假设3秒可以转180度
                        print("右转寻找魔方...")
                        l_pid, r_pid = rotate_in_place_with_pid(pwma, pwmb, 'clockwise', target_speed=1.5, l_pid=l_pid, r_pid=r_pid)
                    else:
                        # 右转完成，继续旋转寻找
                        state.rotation_phase = 'search'
                        print("转向操作完成，继续旋转寻找魔方...")
                else:
                    # 继续旋转寻找
                    l_pid, r_pid = rotate_in_place_with_pid(pwma, pwmb, 'clockwise', target_speed=1, l_pid=l_pid, r_pid=r_pid)
                    
                # 在旋转过程中检测魔方
                found_color = None
                
                # 检查是否有任何颜色的魔方
                for color, segments in color_data.items():
                    if segments:  # 如果检测到该颜色
                        found_color = color
                        break
                
                if found_color:
                    # 颜色确认逻辑
                    if found_color not in color_confirm_counter:
                        color_confirm_counter[found_color] = 1
                    else:
                        color_confirm_counter[found_color] += 1
                    
                    # 重置其他颜色的计数
                    for color in color_confirm_counter:
                        if color != found_color:
                            color_confirm_counter[color] = 0
                    
                    # 检查是否已经连续确认足够次数
                    if color_confirm_counter.get(found_color, 0) >= COLOR_CONFIRM_COUNT:
                        confirmed_color = found_color
                        state_color[3] = confirmed_color  # 记录状态3的颜色
                        print(f"状态3: 确认最后一个魔方颜色为 {confirmed_color}")

        # 控制循环的频率
        time.sleep(0.1)

except KeyboardInterrupt:
    print("程序被用户中断")

finally:
    # 清理资源
    print("清理资源...")
    cleanup_motor(pwma, pwmb)
    cleanup_camera()
    # TODO：添加其他清理代码
    print("程序已安全退出")



